
import Foundation


enum MotorStatus {
    case ok
    case timeout
    case error
}

struct SerialMotorParameters {
    var isServo: Bool = false
    var acc: Int32 = 1200
    var dcc: Int32 = 1200
    var speed: Int32 = 2400
    var steps: Int32 = 0
}

class SerialMotorManager {
    
    private var serialPortManager: SerialPortManager?
    private var motorName: String?
    private var serialMotorParameters: [SerialMotorParameters] = Array.init(repeating: SerialMotorParameters(), count: 255)
    
    private static var messageInterval: Double = 0.015
    private static var timeoutInterval: Double = 1.0
    private static var checkInterval: Double = 0.02
    private let isWaiting: Bool = true
    
    init(motorName: String, numChannels: Int = 255) {
        self.motorName = motorName
        
        self.serialMotorParameters = Array.init(repeating: SerialMotorParameters(), count: numChannels)
        
        connectToMotor(motorName: self.motorName!)
        
    }
    
    func closeManually() {
        serialPortManager?.closeManually()
    }
    
    func isConnected() -> Bool {
        if (self.serialPortManager == nil) {
            return false
        }
        return self.serialPortManager!.isSerialOpened()
    }
    
    func connectToMotor(motorName: String) -> Bool {
        self.serialPortManager = SerialPortManager(isShowingMessage: true)
        self.serialPortManager!.openSerialDevice(serialName: motorName)
        
        return self.isConnected()
    }
    
    func sendCommand(cmd: [UInt8]) {
        self.serialPortManager?.sendUInt8(data: cmd)
        Thread.sleep(forTimeInterval: TimeInterval(SerialMotorManager.messageInterval))
    }
    
    func setAsServo(channel: UInt8, isServo: Bool) {
        serialMotorParameters[Int(channel) - 1].isServo = isServo
    }
    
    func enableMotor(channel: UInt8) -> MotorStatus {
        //01 06 00 7C 00 9F
        var cmd: [UInt8] = [channel]
        cmd.append(0x06)
        if (serialMotorParameters[Int(channel) - 1].isServo) {
            
            cmd.append(0x01)
            cmd.append(0x0A)
            cmd.append(0x00)
            cmd.append(0x08)
        } else {
            cmd.append(0x00)
            cmd.append(0x7C)
            cmd.append(0x00)
            cmd.append(0x9F)
        }
        let crc = calculateCRC16Modbus(input: cmd)
        cmd.append(crc[0])
        cmd.append(crc[1])
        self.sendCommand(cmd: cmd)
        if (isWaiting) {
            var receivedBytes: [UInt8]?
            receivedBytes = getNBytes(numBytes: 8, timeout: SerialMotorManager.timeoutInterval, checkInterval: SerialMotorManager.checkInterval)
            if (receivedBytes == nil) {
                return .timeout
            }
            
            
            return (receivedBytes!.elementsEqual(cmd)) ? .ok : .error
            
        }
        return .ok
    }

    func disableMotor(channel: UInt8) -> MotorStatus {
        //01 06 00 7C 00 9F
        var cmd: [UInt8] = [channel]
        cmd.append(0x06)
        if (serialMotorParameters[Int(channel) - 1].isServo) {
            
                cmd.append(0x01)
                cmd.append(0x0A)
                cmd.append(0x00)
                cmd.append(0x0A)
        } else {
            cmd.append(0x00)
            cmd.append(0x7C)
            cmd.append(0x00)
            cmd.append(0x9E)
        }
        
        let crc = calculateCRC16Modbus(input: cmd)
        cmd.append(crc[0])
        cmd.append(crc[1])
        
        self.sendCommand(cmd: cmd)
        if (isWaiting) {
            var receivedBytes: [UInt8]?
            receivedBytes = getNBytes(numBytes: 8, timeout: SerialMotorManager.timeoutInterval, checkInterval: SerialMotorManager.checkInterval)
            if (receivedBytes == nil) {
                return .timeout
            }
                
            return (receivedBytes!.elementsEqual(cmd)) ? .ok : .error
            
        }
        return .ok
    }
    
    func isMotorStopped(channel: UInt8, timeout: Double = 5) -> Bool? {
        var cmd: [UInt8] = [channel]
        cmd.append(0x03)
        cmd.append(0x00)
        
        if (serialMotorParameters[Int(channel) - 1].isServo) {
            
            cmd.append(0x02)
        } else {
            cmd.append(0x01)
        }
        cmd.append(0x00)
        cmd.append(0x01)
        
        let crc = calculateCRC16Modbus(input: cmd)
        cmd.append(crc[0])
        cmd.append(crc[1])
        
        self.sendCommand(cmd: cmd)
        if (isWaiting) {
            var receivedBytes: [UInt8]?
            receivedBytes = getNBytes(numBytes: 7, timeout: SerialMotorManager.timeoutInterval, checkInterval: SerialMotorManager.checkInterval)
            if (receivedBytes == nil) {
                return nil
            }
            
            return (receivedBytes![4] == 0x09)
        }
        return false
    }
    
    func waitForStop(channel: UInt8, timeout: Double = 5) -> MotorStatus {
        
        let t1 = Date.now.timeIntervalSince1970
        var t2 = Date.now.timeIntervalSince1970
        if (isWaiting) {
            
            while ((t2 - t1) <= ( timeout * 1)) {
                var flag: Bool?
                
                flag = isMotorStopped(channel: channel, timeout: timeout)
                if (flag == nil) {
                    return .timeout
                }
                if (flag! ) {
                    return .ok
                }
                
                t2 = Date.now.timeIntervalSince1970
                
                Thread.sleep(forTimeInterval: TimeInterval(SerialMotorManager.checkInterval))
            }
            return .error
        }
        return .error
    }

    func relativelyMove(channel: UInt8) -> MotorStatus {
        // 01 06 00 7C 00 66
        var cmd: [UInt8] = [channel]
        cmd.append(0x06)
        cmd.append(0x00)
        cmd.append(0x7C)
        cmd.append(0x00)
        cmd.append(0x66)
        
        let crc = calculateCRC16Modbus(input: cmd)
        cmd.append(crc[0])
        cmd.append(crc[1])
        
        self.sendCommand(cmd: cmd)
        if (isWaiting) {
            var receivedBytes: [UInt8]?
            receivedBytes = getNBytes(numBytes: 8, timeout: SerialMotorManager.timeoutInterval, checkInterval: SerialMotorManager.checkInterval)
            if (receivedBytes == nil) {
                return .timeout
            }
            
            return (receivedBytes!.elementsEqual(cmd)) ? .ok : .error
        }
        return .ok
    }

    func homeMotor(channel: UInt8, trigger: UInt8) -> MotorStatus {
        //01 10 00 7C 00 03 06 00 6E 00 31 00 4C 9D 67
        var cmd: [UInt8] = [channel]
        cmd.append(0x10)
        cmd.append(0x00)
        cmd.append(0x7C)
        cmd.append(0x00)
        cmd.append(0x03)
        cmd.append(0x06)
        cmd.append(0x00)
        cmd.append(0x6E)
        cmd.append(0x00)
        cmd.append(trigger)
        cmd.append(0x00)
        cmd.append(0x4C)
        
        let crc = calculateCRC16Modbus(input: cmd)
        cmd.append(crc[0])
        cmd.append(crc[1])
        
        self.sendCommand(cmd: cmd)
        if (isWaiting) {
            var receivedBytes: [UInt8]?
            receivedBytes = getNBytes(numBytes: 8, timeout: SerialMotorManager.timeoutInterval, checkInterval: SerialMotorManager.checkInterval)
            if (receivedBytes == nil) {
                return .timeout
            }
            var cmd_: [UInt8] = []
            for i in 0..<6 {
                cmd_.append(cmd[i])
            }
            let crc_ = calculateCRC16Modbus(input: cmd_)
            cmd_.append(crc_[0])
            cmd_.append(crc_[1])
            
            return (receivedBytes!.elementsEqual(cmd_)) ? .ok : .error
            
        }
        return .ok
    }

    func setAcc(channel: UInt8, acc:Int32)-> MotorStatus {
        // 01 10 00 1B 00 05 0A 02 58 02 58 09 C4 00 07 A1 42
        self.serialMotorParameters[Int(channel) - 1].acc = acc
        return pushStepAndSpeed(channel: channel)
    }
    func setDcc(channel: UInt8, dcc:Int32)-> MotorStatus {
        // 01 10 00 1B 00 05 0A 02 58 02 58 09 C4 00 07 A1 42
        self.serialMotorParameters[Int(channel) - 1].dcc = dcc
        return pushStepAndSpeed(channel: channel)
    }
    func setSteps(channel: UInt8, steps:Int32)-> MotorStatus {
        // 01 10 00 1B 00 05 0A 02 58 02 58 09 C4 00 07 A1 42
        self.serialMotorParameters[Int(channel) - 1].steps = steps
        return pushStepAndSpeed(channel: channel)
    }
    func setSpeed(channel: UInt8, speed:Int32) -> MotorStatus {
        // 01 10 00 1B 00 05 0A 02 58 02 58 09 C4 00 07 A1 42
        self.serialMotorParameters[Int(channel) - 1].speed = speed
        return pushStepAndSpeed(channel: channel)
    }
        
    func pushStepAndSpeed(channel: UInt8)  -> MotorStatus{
        var cmd: [UInt8] = [channel]
        cmd.append(0x10)
        cmd.append(0x00)
        cmd.append(0x1B)
        cmd.append(0x00)
        cmd.append(0x05)
        cmd.append(0x0A)
        
        let _acc = int32ToByteArray(value: serialMotorParameters[Int(channel) - 1].acc)
        cmd.append(_acc[1])
        cmd.append(_acc[0])
        let _dcc = int32ToByteArray(value: serialMotorParameters[Int(channel) - 1].dcc)
        cmd.append(_dcc[1])
        cmd.append(_dcc[0])
        let _speed = int32ToByteArray(value: serialMotorParameters[Int(channel) - 1].speed)
        cmd.append(_speed[1])
        cmd.append(_speed[0])
        
        let _steps = int32ToByteArray(value: serialMotorParameters[Int(channel) - 1].steps)
        
        cmd.append(_steps[3])
        cmd.append(_steps[2])
        cmd.append(_steps[1])
        cmd.append(_steps[0])
        
        let crc = calculateCRC16Modbus(input: cmd)
        cmd.append(crc[0])
        cmd.append(crc[1])
        
        self.sendCommand(cmd: cmd)
        if (isWaiting) {
            var receivedBytes: [UInt8]?
            receivedBytes = getNBytes(numBytes: 8, timeout: SerialMotorManager.timeoutInterval, checkInterval: SerialMotorManager.checkInterval)
            if (receivedBytes == nil) {
                return .timeout
            }
            
            
            var cmd_: [UInt8] = []
            for i in 0..<6 {
                cmd_.append(cmd[i])
            }
            let crc_ = calculateCRC16Modbus(input: cmd_)
            cmd_.append(crc_[0])
            cmd_.append(crc_[1])
            
            
            return (receivedBytes!.elementsEqual(cmd_)) ? .ok : .error
            
        }
        return .ok
    }
    
    func getPosition(channel: UInt8) -> Int32? {
        //03 00 06 00 02
        var cmd: [UInt8] = [channel]
        cmd.append(0x03)
        cmd.append(0x00)
        cmd.append(0x06)
        cmd.append(0x00)
        cmd.append(0x02)
        
        let crc = calculateCRC16Modbus(input: cmd)
        cmd.append(crc[0])
        cmd.append(crc[1])
        
        
        self.sendCommand(cmd: cmd)
        if (isWaiting) {
            var receivedBytes: [UInt8]?
            receivedBytes = getNBytes(numBytes: 9, timeout: 10, checkInterval: SerialMotorManager.checkInterval)
            if (receivedBytes == nil) {
                return nil
            }
            
            
            if (receivedBytes!.count == 9) {
                
                
                let crc_ = calculateCRC16Modbus(input: Array(receivedBytes![0..<7]))
                if (Array(receivedBytes![7..<9]).elementsEqual(crc_)) {
                    
                    return byteArrayToInt32(bytes: Array(receivedBytes![3..<7]))
                }
            }
        }
        return nil
    }
    
    func getErrorCode(channel: UInt8) -> [UInt8]? {
        
        var errorCode: [UInt8] = []
        var cmd: [UInt8] = [channel]
        cmd.append(0x03)
        cmd.append(0x00)
        cmd.append(0x00)
        cmd.append(0x00)
        if (serialMotorParameters[Int(channel) - 1].isServo) {
            
            cmd.append(0x02)
        } else {
            cmd.append(0x01)
        }
        
        let crc = calculateCRC16Modbus(input: cmd)
        cmd.append(crc[0])
        cmd.append(crc[1])
        
        self.sendCommand(cmd: cmd)
        if (isWaiting) {
            var receivedBytes: [UInt8]?
            receivedBytes = getNBytes(numBytes: 7, timeout: SerialMotorManager.timeoutInterval, checkInterval: SerialMotorManager.checkInterval)
            if (receivedBytes == nil) {
                return nil
            }
            
            
            if (receivedBytes!.count == 7) {
                
                
                let crc_ = calculateCRC16Modbus(input: Array(receivedBytes![0..<5]))
                if (Array(receivedBytes![5..<7]).elementsEqual(crc_)) {
                    errorCode.append(receivedBytes![3])
                    errorCode.append(receivedBytes![4])
                    
                    return errorCode
                }
            }
        }
        return nil
    }
    func getNBytes(numBytes: Int, timeout: Double, checkInterval: Double, isShowingResponse: Bool = true) -> [UInt8]? {
        let t1 = Date.now.timeIntervalSince1970
        
        var receivedBytes: [UInt8] = []
        
        while((receivedBytes.count) < numBytes) {
            receivedBytes = (self.serialPortManager?.getInstantReceivedData())!
            
           // print("receivedBytes \(receivedBytes)")
            if (serialPortManager!.stopReading) {
                serialPortManager?.clearInstantReceivedData()
                return nil
            }
            let t2 = Date.now.timeIntervalSince1970
            if ((t2 - t1) > timeout) {
                
                serialPortManager?.clearInstantReceivedData()
                return nil
            }
            Thread.sleep(forTimeInterval: TimeInterval(checkInterval))
            
        }
        receivedBytes =  Array(receivedBytes[0..<numBytes])
        let hexString = receivedBytes.map { String(format: "%02X", $0) }.joined(separator: " ")
        if (isShowingResponse) {
            
                ELog.d(String(describing: type(of: self)), #function, "Message serial in: [\(hexString)]", #line)
                PavaoManager.instance.saveLog(logLevel: LOG_LEVEL.info, logContent: "Message serial in: [\(hexString)]")
        }
        
        serialPortManager?.clearInstantReceivedData()
        return receivedBytes
    }
}
